
#include <physm.h>

extern physm_que_t _physm_sim_que;
extern int _physm_next_tid;

void *_physm_thread_calc_force(void *data){

  //int tid = _physm_next_tid++;
  //fprintf(stderr, "[force-calc] Thread ID: %d\n", tid);

  physm_world_t *world = (physm_world_t *)data;

  physm_body_t *body;
  while (1){
    body = physm_que_next(&_physm_sim_que);
    if (body == NULL){
      break;
    }

    /* Calc the force on this body. */
    _physm_math_calc_force(world, body);
    
  }
  //fprintf(stderr, "[force-calc-%d] Done.\n", tid);
  pthread_exit(NULL);

}

int _physm_thread_run_calc_force(physm_world_t *world, int max){

  /* spawn off 'threads' threads to calculate forces... */
  
  pthread_t threads[max];
  _physm_next_tid = 0;
  physm_que_reset(&_physm_sim_que);

  int i = 0;
  for ( ; i < max; i++){
    pthread_create(&threads[i], NULL, _physm_thread_calc_force, world);
  }
  
  for ( i = 0; i < max; i++){
    pthread_join(threads[i], NULL);
  }

  /* For testing purposes: */
  /*
  i = 0;
  int body_count = physm_llist_len(world->bodies);
  for ( ; i < body_count; i++){
    _physm_body_print(physm_llist_get(world->bodies, i));
  }
  */
  return 0;

}

void *_physm_thread_reloc_bodies(void *data){

  //int tid = _physm_next_tid++;
  //fprintf(stderr, "[reloc-bodies] Thread ID: %d\n", tid);

  physm_world_t *world = (physm_world_t *)data;

  physm_body_t *body;
  while (1){
    body = physm_que_next(&_physm_sim_que);
    if (body == NULL){
      break;
    }

    /* Calc the reloc stuff. */
    _physm_math_reloc_body(world, body);
    
  }

  pthread_exit(NULL);

}

int _physm_thread_run_reloc_bodies(physm_world_t *world, int max){

    pthread_t threads[max];
  _physm_next_tid = 0;
  physm_que_reset(&_physm_sim_que);

  int i = 0;
  for ( ; i < max; i++){
    pthread_create(&threads[i], NULL, _physm_thread_reloc_bodies, world);
  }
  
  for ( i = 0; i < max; i++){
    pthread_join(threads[i], NULL);
  }

  return 0;

}
